//
// Created by Administrator on 2024/6/10.
//

#include "EllipsoidModel.h"

namespace sgt {
    ///M = T * R
    ///V` = T*R*V,其中V为站心坐标系下的点，V`为ECEF坐标系下的点，
    ///由ENU转ECEF时，要先旋转再平移，我理解应该是用很小的数操作比较方便，如果先平移，值就会变的很大;同理由ECEF转ENU会先平移再旋转。
    ///     1	0	0	Xp          −sinL	−sinBcosL	cosBcosL	0
    /// T = 0	1	0	Yp     R =  cosL	−sinBsinL	cosBsinL	0
    ///     0	0	1	Zp          0	    cosB		sinB		0
    ///     0	0	0	1           0	    0		    0		    1
    ///上述矩阵为行主序，将V看做列向量，通过左乘矩阵。
    ///从ECEF 转ENU时，旋转矩阵是正交矩阵，根据正交矩阵的性质:正交矩阵的逆矩阵等于其转置矩阵，那么可直接得：
    ///M−1=R−1∗T−1
    ///         −sinL       cosL        0       0        1	0	0	-Xp
    /// R−1 =   −sinBcosL   −sinBsinL   cosB    0   T-1= 0	1	0	-Yp
    ///         cosBcosL    cosBsinL    sinB    0        0	0	1	-Zp
    ///         0           0           0       1        0	0	0	1
    /// \param X    ECEF坐标点X轴位置
    /// \param Y
    /// \param Z
    /// \param localToWorld
    void EllipsoidModel::computeCoordinateFrame(
            double latitude, double longitude, MATRIX4 &localToWorld) const {
#ifdef MATRIX_EIGEN
        //下面的示例代码介绍了整个变化过程
        //1.将坐标系绕x轴（东向）逆时针旋转pi/2 - latitude ，等价于点旋转矩阵顺时针（右手法则）旋转该角度
        double rxAngle = (PI_2 - latitude);
        Eigen::AngleAxisd rxAngleAxis(rxAngle, Eigen::Vector3d(1, 0, 0));
        Eigen::Matrix3d rX = rxAngleAxis.matrix();

        //2. 坐标系绕Z轴（天向）逆时针旋转pi/2 + longtitude，等价于点旋转矩阵顺时针旋转该角度
        double rzAngle = (longitude +  PI_2);
        Eigen::AngleAxisd rzAngleAxis(rzAngle, Eigen::Vector3d(0, 0, 1));
        Eigen::Matrix3d rZ = rzAngleAxis.matrix();


        //3. 矩阵先重置为单位矩阵，然后将三维矩阵更新到左上角3X3为止上
        localToWorld.setIdentity();
        localToWorld.block<3, 3>(0, 0) = (rZ * rX);     //见矩阵R

        //4.下面的平移，在该函数中不需要的
//        double tx = topocentricOrigin.x();
//        double ty = topocentricOrigin.y();
//        double tz = topocentricOrigin.z();
//        Blh2Xyz(tx, ty, tz);
//        Eigen::Matrix4d translation;
//        translation.setIdentity();
//        translation(0, 3) = tx;
//        translation(1, 3) = ty;
//        translation(2, 3) = tz;
//        resultMat = translation * rotation;


#else
        // Compute east vector
    osg::Vec3d    east    (-sin(longitude), cos(longitude), 0);
        // Compute up vector
    osg::Vec3d    up      ( cos(longitude)*cos(latitude), sin(longitude)*cos(latitude), sin(latitude));

    // Compute north vector = outer product up x east
    osg::Vec3d    north   = up ^ east;      //(-sin(latitude)cos(longitude), -sin(latitude)sin(longitude), cos(latitude));

    // set matrix
    localToWorld(0,0) = east[0];
    localToWorld(0,1) = east[1];
    localToWorld(0,2) = east[2];

    localToWorld(1,0) = north[0];
    localToWorld(1,1) = north[1];
    localToWorld(1,2) = north[2];

    localToWorld(2,0) = up[0];
    localToWorld(2,1) = up[1];
    localToWorld(2,2) = up[2];
#endif
    }

    void
    EllipsoidModel::convertLatLongHeightToXYZ(double latitude, double longitude, double height, double &X, double &Y,
                                              double &Z) const {
        // for details on maths see http://www.colorado.edu/geography/gcraft/notes/datum/gif/llhxyz.gif
        double sin_latitude = sin(latitude);
        double cos_latitude = cos(latitude);
        double N = m_radiusEquator / sqrt(1.0 - m_eccentricitySquared * sin_latitude * sin_latitude);
        X = (N + height) * cos_latitude * cos(longitude);
        Y = (N + height) * cos_latitude * sin(longitude);
        Z = (N * (1 - m_eccentricitySquared) + height) * sin_latitude;
    }

    void EllipsoidModel::convertXYZToLatLongHeight(double X, double Y, double Z, double &latitude, double &longitude,
                                                   double &height) const {
        // handle polar and center-of-earth cases directly.
        if (X != 0.0)
            longitude = atan2(Y, X);
        else {
            if (Y > 0.0)
                longitude = PI_2;
            else if (Y < 0.0)
                longitude = -PI_2;
            else {
                // at pole or at center of the earth
                longitude = 0.0;
                if (Z > 0.0) { // north pole.
                    latitude = PI_2;
                    height = Z - m_radiusPolar;
                } else if (Z < 0.0) { // south pole.
                    latitude = -PI_2;
                    height = -Z - m_radiusPolar;
                } else { // center of earth.
                    latitude = PI_2;
                    height = -m_radiusPolar;
                }
                return;
            }
        }

        // http://www.colorado.edu/geography/gcraft/notes/datum/gif/xyzllh.gif
        double p = sqrt(X * X + Y * Y);
        double theta = atan2(Z * m_radiusEquator, (p * m_radiusPolar));
        double eDashSquared = (m_radiusEquator * m_radiusEquator - m_radiusPolar * m_radiusPolar) /
                              (m_radiusPolar * m_radiusPolar);

        double sin_theta = sin(theta);
        double cos_theta = cos(theta);

        latitude = atan((Z + eDashSquared * m_radiusPolar * sin_theta * sin_theta * sin_theta) /
                        (p - m_eccentricitySquared * m_radiusEquator * cos_theta * cos_theta * cos_theta));

        double sin_latitude = sin(latitude);
        double N = m_radiusEquator / sqrt(1.0 - m_eccentricitySquared * sin_latitude * sin_latitude);

        height = p / cos(latitude) - N;
    }

    void EllipsoidModel::computeLocalToWorldTransformFromLatLongHeight(double latitude, double longitude, double height,
                                                                       MATRIX4 &localToWorld) const {
#ifdef MATRIX_EIGEN
        double X, Y, Z;
        convertLatLongHeightToXYZ(latitude,longitude,height,X,Y,Z);

        MATRIX4 translation;
        translation.setIdentity();
        translation(0, 3) = X;
        translation(1, 3) = Y;
        translation(2, 3) = Z;

        MATRIX4 rotation;
        computeCoordinateFrame(latitude, longitude, rotation);

        localToWorld = translation * rotation;


#else
        double X, Y, Z;
        convertLatLongHeightToXYZ(latitude,longitude,height,X,Y,Z);

        localToWorld.makeTranslate(X,Y,Z);
        computeCoordinateFrame(latitude, longitude, localToWorld);
#endif

    }


    void EllipsoidModel::computeLocalToWorldTransformFromXYZ(double X, double Y, double Z,
                                                             MATRIX4 &localToWorld) const {
#ifdef MATRIX_EIGEN
        double latitude,  longitude,  height;
        convertLatLongHeightToXYZ(latitude,longitude,height,X,Y,Z);

        MATRIX4 translation;
        translation.setIdentity();
        translation(0, 3) = X;
        translation(1, 3) = Y;
        translation(2, 3) = Z;

        MATRIX4 rotation;
        computeCoordinateFrame(latitude, longitude, rotation);

        localToWorld = translation * rotation;

#else
    double  latitude, longitude, height;
    convertXYZToLatLongHeight(X,Y,Z,latitude,longitude,height);
    localToWorld.makeTranslate(X,Y,Z);
    computeCoordinateFrame(latitude, longitude, localToWorld);
#endif

    }

    Vector3 EllipsoidModel::computeLocalUpVector(double X, double Y, double Z) const {
        return Vector3();
    }
} // sgt